#!/usr/bin/env python
#coding=utf-8
# from __future__ import print_function
import roslib
# roslib.load_manifest('my_package')
import sys
import rospy
import random
import argparse
# import imutils
import cv2,os
import numpy as np
from std_msgs.msg import String
from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist,Point
from move_base_msgs.msg import MoveBaseActionGoal
from cv_bridge import CvBridge, CvBridgeError
from PokeTrack import pokemon_tracker

class image_converter:
 
  def __init__(self,tb_indx):
    self.image_pub = rospy.Publisher("/pokemon_go/searcher",Image)
    image_topic = 'tb3_'+tb_indx+"/camera/image_raw"
    vel_topic =  'tb3_'+tb_indx+"/cmd_vel"
    goal_topic = 'tb3_'+tb_indx+"/move_base/goal"
    self.bridge = CvBridge()
    self.static = True  # robot static status
    self.min_vel = 0.02
    self.tb_indx = tb_indx
    print 'subscribe topic: '+image_topic
    
    self.image_sub = rospy.Subscriber(image_topic,Image,self.callback)
    self.PokeTrack = pokemon_tracker(True,tb_indx)
    print('init converter')

    rospy.Subscriber(vel_topic,Twist,self.vel_callback)


    rospy.Subscriber(goal_topic,MoveBaseActionGoal,self.goal_callback)
  # def saveImage(self,event, x, y, flags, param): 
  #   # print(x, y)
  #   if event == cv2.EVENT_LBUTTONDBLCLK:  
  #     print('click and save image')         
  #     self.PokeTrack.savePic()

  def goal_callback(self,ActionGoal):
    goal_point = ActionGoal.goal.target_pose.pose.position
    goal_to_str = "x_{:.3} y_{:.3}".format(goal_point.x,goal_point.y)
    print 'new goal:'+goal_to_str
    self.PokeTrack.cur_goal = goal_to_str

  def vel_callback(self,vel):
    if abs(vel.linear.x) <= self.min_vel and abs(vel.linear.y)  <= self.min_vel and abs(vel.linear.z ) <= self.min_vel \
       and abs(vel.angular.x) <= 0.03 and abs(vel.angular.y) <= 0.03 and abs(vel.angular.z)  <= 0.03\
       and self.static == False:
      self.static = True
      # print vel
      # print 'static'
    else:
      self.static = False

  def callback(self,data):
    # print('invoke call back')
    try:
      image = self.bridge.imgmsg_to_cv2(data, "bgr8")
      self.image = image
      # resized = imutils.resize(image, width=300)
      # cv2.imshow("Image", image)
      # save figure
      # ratio = image.shape[0] / float(resized.shape[0])
      
    except CvBridgeError as e:
      print(e)
    
    image = self.PokeTrack.findPokemon(image,vel_static = self.static)
    # image = imutils.resize(image, width=500)
    cv2.imshow('tb3_'+self.tb_indx, image)

    # cv2.setMouseCallback("image", self.saveImage)  # 鼠标事件的回调函数
    cv2.waitKey(3)

 
def main(args):
  arglen = len(sys.argv)
  if arglen == 1:
    print 'turtlebot index argument required.\nexit\n'
  else:
    ic = image_converter(sys.argv[1])
    rospy.init_node('image_converter', anonymous=True)
    try:
      rospy.spin()
    except KeyboardInterrupt:
      print("Shutting down")
    cv2.destroyAllWindows()
 
if __name__ == '__main__':
    main(sys.argv)
